#include <string>
#include <ostream>
#include <conio.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <windows.h>
#include "PF_LocationApplication.h"

#define TIME_USING to_string((int)(end_time-start_time)*1000/CLOCKS_PER_SEC)


PF_LocationApplication::PF_LocationApplication()
{
}


PF_LocationApplication::~PF_LocationApplication()
{
}

void PF_LocationApplication::appInit()
{
	TRY_START
	
		//read the Config.ini file
	//---------------------------------
	if (!mConfig.ReadConfig(CONFIG_FILE_PATH))
		throw "there can not find config.ini file!";
	readConfig();
	
	//load the map
	//---------------------------------
	map.initMap(mconfig.bin_map_path);

	//load the ground truth
	//---------------------------------
	ground_truth = mPoseGrabber.getGroudTruthOffLine(mconfig.pose_data_path);
	
	//load the observation truth
	//---------------------------------
	if (!mObservation.observationDataInit(mconfig.bin_input_path)) throw "no obsevation file";
	
	//load the action
	//---------------------------------
	mActionAbout.actionGrabberOffLine(ground_truth);

	//init the particle filter
	//---------------------------------
	PF_option pf_option;
	pf_option.noisy = mconfig.noisy;
	pf_option.doParticleShift = mconfig.doParticleShift;
	pf_option.shift_particle_function = mconfig.shift_particle_function;
	pf_option.show_shift_particles = mconfig.show_shift_particles;
	pf_option.particles_num = mconfig.particles_num;
	pf_option.init_area_xy = mconfig.init_area_xy;
	pf_option.init_area_z = mconfig.init_area_z;
	mParticleFilter.pfOptionInit(pf_option);
	mParticleFilter.particleDataInit(ground_truth);
	

	TRY_END

}

void PF_LocationApplication::appRun()
{
	TRY_START
	int display_interval = mconfig.display_interval;
	
	//pcl::visualization::PCLVisualizer viewer("PF Localization");
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	//pcl::PointCloud<PointIoType>::Ptr map_cloud(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr observation(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr particles(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr observation_to_show(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr GT_pose(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB point;
	
	
	//--------viewer and loop control para----------//								
	bool end = false;	//main loop					//
	bool* has_add = new bool();						//
	int adjustment = 1;								//
	//----------------------------------------------//

	//set callback function to refresh the prediction particles
	//----------------------------------------------------------
	*has_add = false;
	mParticleFilter.setCallback(show_prediction_particles,has_add,particles,viewer);

	//----------------------------------------------------------
	//				MAIN LOOP
	//----------------------------------------------------------
	Pose3D action;
	vector<int> using_time;
	vector<Pose3D> log_ground_truth;
	for (size_t i = 0; i<=(ground_truth.size()-1)/ display_interval;i++) {

		observation->clear();
		pcl::copyPointCloud(*mObservation.getFromBin(i * display_interval, mconfig.bin_input_path),*observation);
		/*pcl::VoxelGrid<PointIoType> filter;
		filter.setInputCloud(observation);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*observation);*/
		action = mActionAbout.getActionAddition(i * display_interval, display_interval);

		log_ground_truth.push_back(ground_truth[i * display_interval]);

		LogCat::c(TAG, "------------appRun "+to_string(i)+"----------", action);
		start_time = clock();
		mParticleFilter.executeOn(action,observation, map);
		end_time = clock();
		using_time.push_back((int)(end_time - start_time) * 1000 / CLOCKS_PER_SEC);
		LogCat::c(TAG, "executeOn/ms", TIME_USING);
		LogCat::c(TAG, "After Resampling particles ESS = ",mParticleFilter.ESS());
		//****************************************************************************
		//visualizer code block START
		//****************************************************************************
		point.x = ground_truth[i * display_interval].x;
		point.y = ground_truth[i * display_interval].y;
		point.z = ground_truth[i * display_interval].z;
		point.r = 255;
		point.g = 255;
		point.b = 0;
		GT_pose->push_back(point);
		Pose3D pose(ground_truth[i * display_interval]);
		observation_to_show->clear();
		for (int k = 0; k < observation->size(); k++) {
			pcl::PointXYZRGB pt;
			pose.transformPoint(observation->points[k].x, observation->points[k].y, observation->points[k].z, pt.x, pt.y, pt.z);
			pt.r = 0;
			pt.g = 255;
			pt.b = 0;
			observation_to_show->push_back(pt);
		}
		if (*has_add==false) {
			//add map
			viewer->addPointCloud<PointIoType>(map.getMapData(), "map_cloud");
			//add GT_pose
			viewer->addPointCloud<pcl::PointXYZRGB>(GT_pose, "GT_pose");
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "GT_pose");
			//add particles
			//add the observation_to_show;
			viewer->addPointCloud<pcl::PointXYZRGB>(observation_to_show, "observation_to_show");
			while (!viewer->wasStopped()) {
				viewer->spinOnce(100);
			}
			*has_add = true;
		}
		else {
			//update
			viewer->updatePointCloud(GT_pose, "GT_pose");
			viewer->updatePointCloud(mParticleFilter.getParticlesAsPointCloud(particles), "particles");
			viewer->updatePointCloud(observation_to_show, "observation_to_show");
			viewer->spinOnce(100);
		}
		//****************************************************************************
		//visualizer code block END
		//****************************************************************************	
	}

	mParticleFilter.logOutPut(mconfig.logcat_path, using_time, log_ground_truth);
	mParticleFilter.logOutPutAboutParticles(mconfig.logcat_path);

	LogCat::c(TAG, "------------AppRun Completed------------");

	while (!viewer->wasStopped()) {
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	/*
	while (!end)
	{
		 Finish if ESC is pushed:
		if (_kbhit())
			if ( _getch() == 27) end = true;
		cout << "press!!!!!" << endl;
	}*/

	TRY_END
}

void PF_LocationApplication::readConfig()
{
	mconfig._sequence = mConfig.ReadString("INPUT_OUTPUT", "SEQUENCE", "");
	mconfig.bin_input_path = mConfig.ReadString("INPUT_OUTPUT", "BIN_INPUT_PATH", "");
	mconfig.pcd_input_path = mConfig.ReadString("INPUT_OUTPUT", "PCD_INPUT_PATH", "");
	mconfig.pcd_output_path = mConfig.ReadString("INPUT_OUTPUT", "PCD_OUTPUT_PATH", "");
	mconfig.pcd_map_path = mConfig.ReadString("INPUT_OUTPUT", "PCD_MAP_PATH", "");
	mconfig.bin_map_path = mConfig.ReadString("INPUT_OUTPUT", "BIN_MAP_PATH", "");

	mconfig.pose_data_path = mConfig.ReadString("INPUT_OUTPUT", "POSE_DATA_PATH", "");
	mconfig.logcat_path = mConfig.ReadString("INPUT_OUTPUT", "LOGCAT_PATH", "");

	mconfig.noisy = mConfig.ReadFloat("PF_option", "noisy", 0.0);
	mconfig.doParticleShift= mConfig.ReadInt("PF_option", "doParticleShift", 0);
	mconfig.shift_particle_function = mConfig.ReadInt("PF_option", "shift_particle_function", 0);
	mconfig.show_shift_particles = mConfig.ReadInt("PF_option", "show_shift_particles", 0);
	mconfig.particles_num = mConfig.ReadInt("PF_option", "particles_num", 0);
	mconfig.init_area_xy = mConfig.ReadFloat("PF_option", "init_area_xy", 0.0);
	mconfig.init_area_z = mConfig.ReadFloat("PF_option", "init_area_z", 0.0);
	mconfig.display_interval = mConfig.ReadInt("PF_option", "display_interval", 0);

	mconfig.show_3d = mConfig.ReadInt("SHOW_3D", "show_3d", 0);
	mconfig.show_ground_truth = mConfig.ReadInt("SHOW_3D", "show_ground_truth", 0);
	mconfig.show_ground_truth_with_noisy = mConfig.ReadInt("SHOW_3D", "show_ground_truth_with_noisy", 0);
	mconfig.color_with_z = mConfig.ReadInt("SHOW_3D", "color_with_z", 0);
}


